#ifndef _KALMAN_FILTER_H_
#define _KALMAN_FILTER_H_
#include <Eigen/Dense>
class KalmanFilter
{
public:
    KalmanFilter();
    ~KalmanFilter();

    bool isInitialized();
    void Initialization(Eigen::VectorXd& state_in);
    void Prediction();
    void MeasurementUpdate(const Eigen::VectorXd &z);

    int getState(Eigen::VectorXd &out);

    void setF(const Eigen::MatrixXd& F);
    void setH(const Eigen::MatrixXd& H);
    void setP(const Eigen::MatrixXd& P);
    void setQ(const Eigen::MatrixXd& Q);
    void setR(const Eigen::MatrixXd& R);

protected:
    bool                is_initializated_;
    Eigen::VectorXd     m_state;
    Eigen::MatrixXd     m_F;                  //!< state transistion matrix
    Eigen::MatrixXd     m_P;                  //!< state covariance matrix
    Eigen::MatrixXd     m_Q_;                 //!< process covariance matrix
    Eigen::MatrixXd     m_H;                  //!< measure matrix
    Eigen::MatrixXd     m_R_;                 //!< measure covariance matrix
};

#endif
